package ext.sim.contact.detect;

import context.render.vertexdata.VertexData;
import ext.render.RenderSphere;
import java.util.ArrayList;
import java.util.LinkedList;
import math.mat3;
import math.math;
import math.region.AABB3;
import math.region.Sphere3;
import static math.vec3.*;
import math.vec3;

public class TreeSphere
{
    
    private static final int SPHERES_PER_CHILD = 8;
    
    
    public Node mRoot;
    
    
    public TreeSphere(vec3 pointInBounds, ArrayList<Sphere3> spheres)
    {
        this.mRoot = new Node(pointInBounds, spheres, true);
    }
    
    
    public LinkedList<Node[]> nodePairOverlaps(TreeSphere other)
    {
        LinkedList<Node[]> olNodes = new LinkedList<>();
        this.mRoot.overlap(other.mRoot, olNodes);
        return olNodes;
    }
    
    public boolean overlaps(TreeSphere other)
    {
        return this.mRoot.overlaps(other.mRoot);
    }
    
    
    public void transformToWorldSpace(mat3 R, vec3 T)
    {
        this.mRoot.transform(R, T);
    }
    
    public void transformToBodySpace()
    {
        this.mRoot.transformToBodySpace();
    }
    
    public void render(VertexData aVertexData)
    {
        this.mRoot.render(aVertexData);
    }
    
    
    
    
    
    
    public class Node
    {
        
        public void allChild(LinkedList<Node> list)
        {
            if (this.isChild())
            {
                list.addLast(this);
            }
            else
            {
                for (Node c : this.mChild)
                {
                    c.allChild(list);
                }
            }
        }
        
        public ArrayList<Sphere3> mSpheres;
        public ArrayList<vec3> mSphereOrig_B;
        
        public Sphere3 mBounds;
        public vec3 mBoundsOrig_B;
        
        public ArrayList<Node> mChild;
        
        public boolean mIsHead;
        
        
        public Node(vec3 pointInBounds, ArrayList<Sphere3> aSpheres, boolean aIsHead)
        {
            this.mIsHead = aIsHead;
            
            AABB3 aabbBounds = new AABB3(pointInBounds, pointInBounds);
            for (Sphere3 s : aSpheres)
            {
                aabbBounds.extendToContain(sub(s.mOrig, 1.1 * s.mRadius));
                aabbBounds.extendToContain(add(s.mOrig, 1.1 * s.mRadius));
            }
            
            this.mBounds = new Sphere3();
            {
                vec3 meanX = new vec3();
                for (Sphere3 s : aSpheres)
                {
                    meanX.add(s.mOrig);
                }
                meanX.div(aSpheres.size());
                this.mBounds.mOrig.set(meanX);
                
                double maxR = 0.0;
                for (Sphere3 s : aSpheres)
                {
                    maxR = math.max(maxR, distance(meanX, s.mOrig) + 1.1 * s.mRadius);
                }
                this.mBounds.mRadius = maxR;
            }
            this.mBoundsOrig_B = this.mBounds.mOrig.copy();
            
            if (aSpheres.size() <= SPHERES_PER_CHILD || aIsHead)
            {
                this.mSpheres = aSpheres;
                this.mSphereOrig_B = new ArrayList<>();
                for (int i = 0; i < aSpheres.size(); i++)
                {
                    Sphere3 s = aSpheres.get(i);
                    this.mSphereOrig_B.add(i, s.mOrig.copy());
                }
            }
            else
            {
                vec3 full = aabbBounds.size();
                vec3 half = div(full, 2.0);
                
                AABB3 boxLLL = new AABB3(aabbBounds.mMin, aabbBounds.mMin); // MIN-NEIGHBORS
                boxLLL.mMax.add(half);
                
                AABB3 boxRLL = new AABB3(aabbBounds.mMin, aabbBounds.mMin);
                boxRLL.mMin.x += half.x;
                boxRLL.mMax.add(half);
                boxRLL.mMax.x += half.x;
                
                AABB3 boxLRL = new AABB3(aabbBounds.mMin, aabbBounds.mMin);
                boxLRL.mMin.y += half.y;
                boxLRL.mMax.add(half);
                boxLRL.mMax.y += half.y;
                
                AABB3 boxLLR = new AABB3(aabbBounds.mMin, aabbBounds.mMin);
                boxLLR.mMin.z += half.z;
                boxLLR.mMax.add(half);
                boxLLR.mMax.z += half.z;
                
                AABB3 boxLRR = new AABB3(aabbBounds.mMax, aabbBounds.mMax); // MAX-NEIGHBORS
                boxLRR.mMin.sub(half);
                boxLRR.mMin.x -= half.x;
                boxLRR.mMax.x -= half.x;
                
                AABB3 boxRLR = new AABB3(aabbBounds.mMax, aabbBounds.mMax);
                boxRLR.mMin.sub(half);
                boxRLR.mMin.y -= half.y;
                boxRLR.mMax.y -= half.y;
                
                AABB3 boxRRL = new AABB3(aabbBounds.mMax, aabbBounds.mMax);
                boxRRL.mMin.sub(half);
                boxRRL.mMin.z -= half.z;
                boxRRL.mMax.z -= half.z;
                
                AABB3 boxRRR = new AABB3(aabbBounds.mMax, aabbBounds.mMax);
                boxRRR.mMin.sub(half);
                
                ArrayList<Sphere3> blocksLLL = new ArrayList<>();
                ArrayList<Sphere3> blocksLLR = new ArrayList<>();
                ArrayList<Sphere3> blocksLRL = new ArrayList<>();
                ArrayList<Sphere3> blocksLRR = new ArrayList<>();
                ArrayList<Sphere3> blocksRLL = new ArrayList<>();
                ArrayList<Sphere3> blocksRLR = new ArrayList<>();
                ArrayList<Sphere3> blocksRRL = new ArrayList<>();
                ArrayList<Sphere3> blocksRRR = new ArrayList<>();
                
                for (Sphere3 s : aSpheres)
                {
                    if (this.overlapSphere_AABB(s, boxLLL))     blocksLLL.add(s);
                    if (this.overlapSphere_AABB(s, boxLLR))     blocksLLR.add(s);
                    if (this.overlapSphere_AABB(s, boxLRL))     blocksLRL.add(s);
                    if (this.overlapSphere_AABB(s, boxLRR))     blocksLRR.add(s);
                    if (this.overlapSphere_AABB(s, boxRLL))     blocksRLL.add(s);
                    if (this.overlapSphere_AABB(s, boxRLR))     blocksRLR.add(s);
                    if (this.overlapSphere_AABB(s, boxRRL))     blocksRRL.add(s);
                    if (this.overlapSphere_AABB(s, boxRRR))     blocksRRR.add(s);
                }
                
                this.mChild = new ArrayList<>();
                {
                    this.mChild.add(new Node(boxLLL.midpoint(), blocksLLL, false));
                    this.mChild.add(new Node(boxLLL.midpoint(), blocksLLR, false));
                    this.mChild.add(new Node(boxLRL.midpoint(), blocksLRL, false));
                    this.mChild.add(new Node(boxLRL.midpoint(), blocksLRR, false));
                    this.mChild.add(new Node(boxRLL.midpoint(), blocksRLL, false));
                    this.mChild.add(new Node(boxRLL.midpoint(), blocksRLR, false));
                    this.mChild.add(new Node(boxRRL.midpoint(), blocksRRL, false));
                    this.mChild.add(new Node(boxRRL.midpoint(), blocksRRR, false));
                }
            }
        }
        
        
        public void overlap(Node other, LinkedList<Node[]> list)
        {
            boolean overlaps = this.mBounds.overlaps(other.mBounds);
            if (!overlaps)
            {
                return;
            }
            boolean thisChild = this.isChild();
            boolean otherChild = other.isChild();
            if (thisChild && otherChild)
            {
                list.add(new Node[] { this, other });
            }
            else if (thisChild)
            {
                for (Node c : other.mChild)
                {
                    this.overlap(c, list);
                }
            }
            else if (otherChild)
            {
                for (Node c : this.mChild)
                {
                    c.overlap(other, list);
                }
            }
            else
            {
                for (Node c : this.mChild)
                {
                    for (Node d : other.mChild)
                    {
                        c.overlap(d, list);
                    }
                }
            }
        }
        
        public boolean overlaps(Node other)
        {
            boolean overlaps = this.mBounds.overlaps(other.mBounds);
            if (!overlaps)
            {
                return false;
            }
            boolean thisChild = this.isChild();
            boolean otherChild = other.isChild();
            if (thisChild && otherChild)
            {
                return true;
            }
            else if (thisChild)
            {
                for (Node c : other.mChild)
                {
                    if (this.overlaps(c))
                    {
                        return true;
                    }
                }
                return false;
            }
            else if (otherChild)
            {
                for (Node c : this.mChild)
                {
                    if (c.overlaps(other))
                    {
                        return true;
                    }
                }
                return false;
            }
            for (Node c : this.mChild)
            {
                for (Node d : other.mChild)
                {
                    if (c.overlaps(d))
                    {
                        return true;
                    }
                }
            }
            return false;
        }
        
        
        
        public boolean isChild()
        {
            return this.mChild == null;  //  <=>  this.mSpheres == null
        }
        
        
        
        public void transform(mat3 R, vec3 T)
        {
            if (this.mIsHead)
            {
                for (int i = 0; i < this.mSpheres.size(); i++)
                {
                    Sphere3 s = this.mSpheres.get(i);
                    vec3 orig_b = this.mSphereOrig_B.get(i);
                    
                    s.mOrig.set(R.mul(orig_b));
                    s.mOrig.add(T);
                }
            }
            
            this.mBounds.mOrig.set(R.mul(this.mBoundsOrig_B));
            this.mBounds.mOrig.add(T);
            
            if (!this.isChild())
            {
                for (Node c : this.mChild)
                {
                    c.transform(R, T);
                }
            }
        }
        
        public void transformToBodySpace()
        {
            if (this.mIsHead)
            {
                for (int i = 0; i < this.mSpheres.size(); i++)
                {
                    Sphere3 s = this.mSpheres.get(i);
                    vec3 orig_b = this.mSphereOrig_B.get(i);
                    
                    s.mOrig.set(orig_b);
                }
            }
            
            this.mBounds.mOrig.set(this.mBoundsOrig_B);
            
            if (!this.isChild())
            {
                for (Node c : this.mChild)
                {
                    c.transformToBodySpace();
                }
            }
        }
        
        
        
        public void render(VertexData aVertexData)
        {
            if (this.isChild())
            {
                for (Sphere3 s : this.mSpheres)
                {
                    RenderSphere.render(aVertexData, s.mOrig, mat3.IDENTITY, s.mRadius);
                }
//                RenderSphere.render(aVertexData, this.mBounds.mOrig, mat3.IDENTITY, this.mBounds.mRadius);
            }
            else
            {
                for (Node c : this.mChild)
                {
                    c.render(aVertexData);
                }
            }
        }
        
        
        
        
        
        private boolean overlapSphere_AABB(Sphere3 s, AABB3 aabb)
        {
            vec3 closestPt = s.mOrig.copy();
            closestPt.clamp(aabb.mMin, aabb.mMax);
            double sqDist = distanceSq(closestPt, s.mOrig);
            double sqRadi = s.mRadius * s.mRadius;
            return sqDist <= sqRadi;
        }
        
    }
    
}
